package demo.miner;

import java.awt.Color;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.LinkedList;
import java.util.List;

import jtech.Drawer;
import jtech.math.Vec2f;

public class AStar {

	private boolean[][] mapGridPassable;
	private Vec2f[] mapGridPoint;
	private int mapGridWidth;
	private int mapGridHeight;
	private int mapGridSize;
	
    private float[] gCosts;
    private float[] fCosts;
    private int[] navigated;
    
    private List<Integer> closedset;
    private List<Integer> openset;
    
    private boolean avoidCorners = true;
    
    private final Comparator<Integer> sort = new Comparator<Integer>() {

        @Override
        public int compare(Integer a, Integer b) {
            if (fCosts[a] < fCosts[b]) {
                return -1;
            } else if (fCosts[a] > fCosts[b]) {
                return 1;
            }
            return 0;
        }

    };
    
    public AStar(boolean[][] mapGridPassable, int mapGridSize) {
    	this.mapGridPassable = mapGridPassable;
    	this.mapGridSize = mapGridSize;
    	
    	mapGridHeight = mapGridPassable.length;
    	mapGridWidth = mapGridPassable[0].length;
    	
    	int len = mapGridHeight * mapGridWidth;
        gCosts = new float[len];
        fCosts = new float[len];
        navigated = new int[len];
        
        closedset = new ArrayList<Integer>();
        openset = new ArrayList<Integer>();
        
        mapGridPoint = new Vec2f[len];
        for (int y = 0; y < mapGridHeight; y++) {
        	for (int x = 0; x < mapGridWidth; x++) {
        		float vx = mapGridSize / 2f + (x * mapGridSize);
        		float vy = mapGridSize / 2f + (y * mapGridSize);
        		mapGridPoint[y * mapGridWidth + x] = new Vec2f(vx, vy);
        	}
        }
        
    }
    
    private float distance(int from, int to) {
        Vec2f a = mapGridPoint[from];
        Vec2f b = mapGridPoint[to];
        return a.distance(b);
    }

    private List<Vec2f> reconstructPath(int currentNode) {
        LinkedList<Vec2f> path = new LinkedList<Vec2f>();
        path.addFirst(mapGridPoint[currentNode].copy());
        
        currentNode = navigated[currentNode];
        while (currentNode != -1) {
            path.addFirst(mapGridPoint[currentNode].copy());
            currentNode = navigated[currentNode];
        }
        
        return path;
    }
    
    private boolean isValidGrid(int x, int y) {
        return (x >= 0 && x < mapGridWidth && y >= 0 && y < mapGridHeight);
    }
    
    private boolean isValidLocation(int current, int neighbor) {
        int sx = current % mapGridWidth;
        int sy = current / mapGridWidth;
        
        int x = neighbor % mapGridWidth;
        int y = neighbor / mapGridWidth;
        
        boolean valid = isValidGrid(x, y) && mapGridPassable[y][x];
        
        if (valid && avoidCorners) {
            if (x-sx != 0 && y-sy != 0) {
                valid = mapGridPassable[sy][x] && mapGridPassable[y][sx];
            }
        }
        
        return valid;
    }
    
    private List<Integer> getNeighbours(int idx) {
		int x = idx % mapGridWidth;
		int y = idx / mapGridWidth;
        List<Integer> neighbours = new ArrayList<Integer>();
        for (int i = -1; i < 2; i++) {
            for (int j = -1; j < 2; j++) {

                int cx = x + i;
                int cy = y + j;

                // skip if equal to this node
                if ((i == 0) && (j == 0)) {
                    continue;
                }

                if (!isValidGrid(cx, cy) || !mapGridPassable[cy][cx]) {
                    continue;
                }

                neighbours.add(cy * mapGridWidth + cx);

            }
        }

        return neighbours;
    }
    
    public List<Vec2f> search(int start, int goal) {
    	
        if (start == goal) {
            List<Vec2f> path = new ArrayList<Vec2f>(0);
            path.add(mapGridPoint[goal]);
            return path;
        }
    	
        
        closedset.clear();
        openset.clear();

        Arrays.fill(gCosts, 0);
        Arrays.fill(fCosts, 0);
        Arrays.fill(navigated, -1);
        
        gCosts[start] = 0;
        fCosts[start] = gCosts[start] + distance(start, goal);

        openset.add(start);
        
        while (!openset.isEmpty()) {
        	
        	Collections.sort(openset, sort);
            
            int current = openset.remove(0);
            
            if (current == goal) {
                return reconstructPath(goal);
            }
            
            closedset.add(current);

            for (int neighbor : getNeighbours(current)) {
                
                if (closedset.contains(neighbor)) {
                    continue;
                }
                
                if (!isValidLocation(current, neighbor)) {
                    continue;
                }
                
                float gCost = gCosts[current] + distance(current, neighbor);
                
                if (!openset.contains(neighbor) || gCost < gCosts[neighbor]) {
                    navigated[neighbor] = current;
                    gCosts[neighbor] = gCost;
                    fCosts[neighbor] = gCosts[neighbor] + distance(neighbor, goal);
                    if (!openset.contains(neighbor)) {
                        openset.add(neighbor);
                    }
                }
                
            }
            
        }
        
        return new ArrayList<Vec2f>(0);
        
    }
    
    public List<Vec2f> search(Vec2f start, Vec2f goal) {
    	int startx = (int) start.x / mapGridSize;
    	int starty = (int) start.y / mapGridSize;
    	int goalx = (int) goal.x / mapGridSize;
    	int goaly = (int) goal.y / mapGridSize;
    	int from = starty * mapGridWidth + startx;
        int to  = goaly * mapGridWidth + goalx;
        return search(from, to);
    }

    public void debug(Drawer drawer, boolean drawNodeIDs, boolean drawEdges) {

    	drawer.lineWidth1();
        for (int y = 0; y < mapGridHeight; y++) {
        	for (int x = 0; x < mapGridWidth; x++) {
        		int idx = y * mapGridWidth + x; 
        		float sx = x * mapGridSize;
        		float sy = y * mapGridSize;
        				
	            if (!mapGridPassable[y][x]) {
	                drawer.color(255, 0, 0, 48);
	                drawer.rectf(sx, sy, mapGridSize, mapGridSize);
	            }
	            
	            drawer.color(Color.BLUE).rect(sx, sy, mapGridSize, mapGridSize);
	
	            drawer.color(200, 200, 200, 128);
	            drawer.circlef(mapGridPoint[idx].x, mapGridPoint[idx].y, 2);
	
	            drawer.color(200, 200, 200, 255);
	            if (drawNodeIDs) {
	                drawer.printc(mapGridPoint[idx].x, mapGridPoint[idx].y - 3, "%d", idx);
	            }
	
	            drawer.color(200, 200, 200, 48);
	            if (drawEdges) {
	                if (!mapGridPassable[y][x]) {
	                    continue;
	                }
	                for (int j : getNeighbours(idx)) {
	                    drawer.line(mapGridPoint[idx].x, (float)mapGridPoint[idx].y, mapGridPoint[j].x, mapGridPoint[j].y);
	                }
	            }
	
	        }
    	}

    }
       
}
